#include "can.h"
#include "cmsis_os2.h"

#include "oslib.h"

#include "shooter.h"

#define ENCODER1_ID 0xEC
#define ENCODER2_ID 0xED

extern osMessageQueueId_t EncoderExQueueHandle;

void shooter_encoder_ex_task(void *argument)
{
    for (;;) 
    {
        static CAN_ConnMessage msg;
        osMessageQueueGet(EncoderExQueueHandle, &msg, NULL, osWaitForever);
        if (msg.id == ENCODER1_ID) {
            shooter_force_update(shooter1, msg.payload.in[0]);
        }
        // else if (msg.id == ENCODER2_ID) {
        //     shooter_force_update(shooter2, msg.payload.in);
        // }
    }
}